#include "hardware_interface/hardware_interface.h"

HardwareInterface::HardwareInterface(){
    // 构造函数实现，初始化成员变量等操作
}

HardwareInterface::~HardwareInterface() {
    // 析构函数实现，释放资源等操作
    if (__stm32_serial.isOpen()) {
        __stm32_serial.close();
    }
}

/**
 * @brief 初始化串口通信
 *
 * 该函数用于初始化与STM32微控制器的串口通信。
 *
 * @param port 串口号，字符串类型，表示串口设备路径
 * @param baudrate 波特率，整数类型，表示通信速率
 *
 * @return 如果串口初始化成功，返回true；否则返回false
 */
bool HardwareInterface::SerialInit(const std::string & port, int baudrate) {

    try{
        __stm32_serial.setPort(port);
        __stm32_serial.setBaudrate(baudrate);
        serial::Timeout _timeout = serial::Timeout::simpleTimeout(2000);//设置超时时间
        __stm32_serial.setTimeout(_timeout);
        __stm32_serial.open();

        if(__stm32_serial.isOpen()){
            std::cout << "[Hardware] Serial opened.\n";
            return true;
        } 
        else {
            std::cerr << "[Hardware] Serial open failed.\n";
            return false;
        }
    } 
    catch (const std::exception& e) {
        std::cerr << "[Hardware] Exception: " << e.what() << "\n";
        return false;
    }
    
}

// ============================= 上行处理 =================================

/**
 * @brief 从硬件接口获取串口数据
 *
 * 从STM32串口读取数据，处理并存储到接收缓冲区，同时解析有效帧。
 *
 * @return 如果找到并解析了有效帧，则返回true；否则返回false。
 */
bool HardwareInterface::GetSerialData(void) {
    if (!__stm32_serial.isOpen()) return false;

    // 1. 动态读取可用数据（非固定字节）
    size_t available = __stm32_serial.available();
    if (available > 0) {
        // 限制单次读取量防止缓冲区膨胀
        size_t read_size = std::min(available, static_cast<size_t>(256));
        std::vector<uint8_t> temp(read_size);//临时缓冲区，用于读取数据,预分配内存空间，提高效率。
        size_t bytes_read = __stm32_serial.read(temp.data(),read_size);
        temp.resize(bytes_read); // 调整临时缓冲区大小以匹配实际读取的字节数（可选，取决于是否需要精确控制内存使用)
        __recv_buffer.insert(__recv_buffer.end(), temp.begin(), temp.end());
    }
    //打印调试信息__recv_buffer
    // if(__recv_buffer.size() > 0){
    //     std::cout << "[Hardware] Received data: ";
    //     for(auto &byte : __recv_buffer) {
    //         printf("%x ", byte);
    //     }
    //     std::cout << std::endl;
    // }
    bool frame_found = false;
    size_t processed_bytes = 0;  // 已处理字节计数
    size_t max_frames_per_call = 5; // 防止单次处理过多帧
    const size_t buffer_size = __recv_buffer.size();

    // 2. 高效帧处理（带处理上限）
    while (buffer_size - processed_bytes >= FRAME_MIN_LEN && max_frames_per_call > 0) {
        
        // std::cout << "[Hardware] Processing frame at index: " << processed_bytes << std::endl;
        const size_t start_index = processed_bytes;
        
        // 2.1 快速跳过非帧头数据
        if (start_index + 1 >= buffer_size || 
            __recv_buffer[start_index] != FRAME_HEADER_1 || 
            __recv_buffer[start_index + 1] != FRAME_HEADER_2) {
            processed_bytes += 1;
            continue;
        }

        // 2.2 检查帧长度有效性
        if (start_index + 2 >= buffer_size) break;
        const uint8_t data_length = __recv_buffer[start_index + 2]; // 帧数据长度（不包括帧头、尾, CMD + data + CRC）

        // 2.3 计算完整帧长（基于协议定义）
        const size_t frame_len = FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + data_length + FRAME_FOOTER_SIZE;// 最小帧长检查（帧头(2)+Length(1)+CmdID(1)+CRC(1)+帧尾(1)=6）
          
        // 2.4 帧完整性检查（包含帧尾位置）
        if (start_index + frame_len > buffer_size) break;

        // 2.5 帧尾验证
        const size_t footer_pos = start_index + frame_len - 1;
        if (__recv_buffer[footer_pos] != FRAME_FOOTER) {
            processed_bytes += 2; // 仅跳过帧头
            continue;
        }
      
        // 2.6 提取帧数据
      
        auto frame_start = __recv_buffer.begin() + start_index;
        auto frame_end = frame_start + frame_len;

        std::vector<uint8_t> frame_data(frame_start, frame_end);

        // 打印调试信息
        // std::cout << "[Hardware] Parsing frame: ";
        // for(auto &byte : frame_data) {
        //     printf("%x ", byte);
        // }
        // std::cout << std::endl;

        // 2.7 解析帧
        if (ParseRecvBuffer(frame_data)) {
            frame_found = true;
        }

        // 2.8 更新处理位置
        max_frames_per_call--;
        processed_bytes += frame_len;
    }

    // 3. 清理已处理数据
    if (processed_bytes > 0) {
        __recv_buffer.erase(__recv_buffer.begin(), 
                          __recv_buffer.begin() + processed_bytes);
    }

    // 4. 防止缓冲区无限增长
    if (__recv_buffer.size() > BUFFER_MAX_SIZE) {
        // 清理缓冲区尾部多余数据，保留最新512字节
        size_t excess = __recv_buffer.size() - BUFFER_SAFE_SIZE;
        __recv_buffer.erase(__recv_buffer.begin(), 
                          __recv_buffer.begin() + excess);
    }

    return frame_found;
}

/**
 * @brief 解析接收缓冲区并更新相应状态
 *
 * 解析传入的 valid_frame 数据，并根据其中的内容更新相应状态。
 *
 * @param valid_frame 接收到的有效帧数据
 * @return 解析成功返回 true，否则返回 false
 */
bool HardwareInterface::ParseRecvBuffer(const std::vector<uint8_t>& valid_frame){
    // 解析 valid_frame 并更新相应状态
    // 检查帧是否为空
    if (valid_frame.empty()) {
        return false;
    }
    
    if (valid_frame.size() < FRAME_MIN_LEN) return false;

    // uint8_t data_length = valid_frame[2];
    uint8_t cmd_id = valid_frame[3];
    const std::vector<uint8_t> data_payload(valid_frame.begin() + 4, valid_frame.end() - 2); // data
    uint8_t crc = valid_frame[valid_frame.size() - 2];

    // CRC 校验
    if (!CheckCRC(std::vector<uint8_t>(valid_frame.begin() + 3, valid_frame.end() - 2), crc)) {
        std::cerr << "[Hardware] CRC check failed.\n";
        return false;
    }

    switch (cmd_id) {
        case CMD_STATE_BLADE:
            // 解析刀盘状态
            HandleBlade(data_payload);
            break;
        case CMD_STATE_BATTERY:
            // 解析电池电压，并缓存发布用
            break;
        case CMD_ENCODER:
            // 解析编码器值
            HandleEncoder(data_payload);
            break;
        default:
            // 未知指令
            return false;
    }

    return true;
}


void HardwareInterface::HandleEncoder(const std::vector<uint8_t>& data) {
    if (data.size() < 8) {
        std::cerr << "[Hardware] Encoder frame too short.\n";
        return;
    }

    __motor_encoder_counts.left_mm = ParseInt32BE(data, 0);
    __motor_encoder_counts.right_mm = ParseInt32BE(data, 4);
    // __motor_encoder_counts.timestamp = rclcpp::Clock().now();

    // std::cout << "[Hardware] Encoder L: " << __motor_encoder_counts.left_mm
    //           << ", R: " << __motor_encoder_counts.right_mm << "\n";
}

void HardwareInterface::HandleBlade(const std::vector<uint8_t>& data){
    __hardware_states.cutter_spd_state = data[0];
}



// ============================ 下发指令 =============================


/**
 * @brief 发送速度控制命令
 *
 * 该函数用于向硬件接口发送速度控制命令，控制左右轮的速度。
 *
 * @param left_mps 左轮速度，单位：米/秒
 * @param right_mps 右轮速度，单位：米/秒
 */
void HardwareInterface::SendVelocityCommand(double left_mps, double right_mps) {
    if(!__stm32_serial.isOpen()) return;

    constexpr uint8_t DATA_LEN = 0x06;  // CMD(1) + L(2) + R(2) + CRC(1)

    int16_t left_speed = static_cast<int16_t>(left_mps * 1000);   // 单位 mm/s
    int16_t right_speed = static_cast<int16_t>(right_mps * 1000); // 单位 mm/s

    std::vector<uint8_t> data;
    data.reserve(10); // header(2) + length(1) + data(6) + tail(1)

    data.emplace_back(FRAME_HEADER_1);  // 0x55
    data.emplace_back(FRAME_HEADER_2);  // 0xAA
    data.emplace_back(DATA_LEN);            // Length: 从 CMD 到 CRC = 6字节
    data.emplace_back(CMD_ENCODER);     // 0x01 速度控制命令

    // 左轮速度，高字节在前
    // 右轮速度，高字节在前
    AppendInt16BigEndian(data, left_speed);
    AppendInt16BigEndian(data, right_speed);

    // CRC 简单异或校验 （CmdID + Data）
    uint8_t crc = CalculateCRC(
        data.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE, 
        data.end()
    );
    
    data.emplace_back(crc);
     
    data.emplace_back(FRAME_FOOTER);   // 0xAA

    // 打印调试信息
    // std::cout << "[Hardware] Sending speed command: ";
    // for(auto &byte : data) {
    //     printf("%x ", byte);
    // }
    // std::cout << std::endl;

    WriteData(data);
}


void HardwareInterface::SendCutterSpdCtlCmd(int8_t cutter_spd) {
    if(!__stm32_serial.isOpen()) return;

    constexpr uint8_t DATA_LEN = 0x04; // CmdID + Data(2字节) + CRC
    constexpr uint8_t RESERVED = 0x00;
   
    std::vector<uint8_t> data;
    data.reserve(8); // 提前分配空间，减少后续扩容次数

    // 帧头
    data.emplace_back(FRAME_HEADER_1);  // 0x55
    data.emplace_back(FRAME_HEADER_2);  // 0xAA

    // 长度字段，此处为4字节（包括CRC和帧尾）
    data.push_back(DATA_LEN);            // Length: 从 CMD 到 CRC = 4字节
    
    // 命令ID
    data.push_back(CMD_BLADE);     // 0x02 速度控制命令

    // 刀盘速度（1字节），后面加1字节保留位
    data.emplace_back(static_cast<uint8_t>(cutter_spd));
    data.emplace_back(RESERVED);

    // CRC 简单异或校验 （从 CmdID 到 Data），不含头和尾
    uint8_t crc = CalculateCRC(
        data.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE, 
        data.end()
    );
    data.emplace_back(crc);
    
    // 帧尾
    data.emplace_back(FRAME_FOOTER);   // 0xAA

    // 打印调试信息
    // std::cout << "[Hardware] Sending cutter speed control command: ";
    // for(auto &byte : data) {
    //     printf("%x ", byte);
    // }
    // std::cout << std::endl;

    WriteData(data);
}



void HardwareInterface::SendGetCutterSpdCmd(void) {
    if(!__stm32_serial.isOpen()) return;

    constexpr uint8_t DATA_LEN = 0x02; // CmdID + Data(0字节) + CRC

    std::vector<uint8_t> data;
    data.reserve(6); // 提前分配空间，减少后续扩容次数

    // 帧头
    data.emplace_back(FRAME_HEADER_1);  // 0x55
    data.emplace_back(FRAME_HEADER_2);  // 0xAA

    // 长度字段，此处为2字节（包括CRC和帧尾）
    data.push_back(DATA_LEN);            // Length: 从 CMD 到 CRC = 4字节
    
    // 命令ID
    data.push_back(CMD_STATE_BLADE);     // 0x70 刀盘状态

    // CRC 简单异或校验 （从 CmdID 到 Data），不含头和尾
    uint8_t crc = CalculateCRC(
        data.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE, 
        data.end()
    );
    data.emplace_back(crc);
    
    // 帧尾
    data.emplace_back(FRAME_FOOTER);   // 0xAA

    // 打印调试信息
    // std::cout << "[Hardware] Sending cutter speed control command: ";
    // for(auto &byte : data) {
    //     printf("%x ", byte);
    // }
    // std::cout << std::endl;

    WriteData(data);
}


void HardwareInterface::SendGetBatStateCmd(void) {
    if(!__stm32_serial.isOpen()) return;

    constexpr uint8_t DATA_LEN = 0x02; // CmdID + Data(0字节) + CRC

    std::vector<uint8_t> data;
    data.reserve(6); // 提前分配空间，减少后续扩容次数

    // 帧头
    data.emplace_back(FRAME_HEADER_1);  // 0x55
    data.emplace_back(FRAME_HEADER_2);  // 0xAA

    // 长度字段，此处为2字节（包括CRC和帧尾）
    data.push_back(DATA_LEN);            // Length: 从 CMD 到 CRC = 4字节
    
    // 命令ID
    data.push_back(CMD_STATE_BATTERY);     // 0x71 刀盘状态

    // CRC 简单异或校验 （从 CmdID 到 Data），不含头和尾
    uint8_t crc = CalculateCRC(
        data.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE, 
        data.end()
    );
    data.emplace_back(crc);
    
    // 帧尾
    data.emplace_back(FRAME_FOOTER);   // 0xAA

    // 打印调试信息
    // std::cout << "[Hardware] Sending cutter speed control command: ";
    // for(auto &byte : data) {
    //     printf("%x ", byte);
    // }
    // std::cout << std::endl;

    WriteData(data);
}


/**
 * @brief 向硬件接口写入数据
 *
 * 该函数通过STM32串口向硬件接口写入数据。
 *
 * @param data 要写入的数据，类型为uint8_t类型的vector容器
 * @return 如果数据成功写入，则返回true；否则返回false
 */
bool HardwareInterface::WriteData(const std::vector<uint8_t>& data) {
    if (!__stm32_serial.isOpen()) return false;
    size_t written = __stm32_serial.write(data);
    return written == data.size();
}


// ============================ 工具函数 ===============================


/**
 * @brief 向缓冲区中追加一个大端模式的16位整数
 *
 * 将给定的16位整数以大端模式追加到提供的字节缓冲区中。
 *
 * @param buf 字节缓冲区
 * @param val 要追加的16位整数
 */
inline void HardwareInterface::AppendInt16BigEndian(std::vector<uint8_t>& buf, int16_t val) {
    buf.push_back(static_cast<uint8_t>((val >> 8) & 0xFF)); // High byte
    buf.push_back(static_cast<uint8_t>(val & 0xFF));        // Low byte
}


/**
 * @brief 从给定的字节序列中解析出大端序的32位整数
 *
 * 从提供的字节序列中，从指定的偏移量开始，解析出大端序的32位整数。
 *
 * @param data 包含字节序列的向量
 * @param offset 字节序列中的起始偏移量
 *
 * @return 解析出的大端序32位整数
 */
int32_t HardwareInterface::ParseInt32BE(const std::vector<uint8_t>& data, size_t offset) {
    return (static_cast<int32_t>(data[offset]) << 24) |
           (static_cast<int32_t>(data[offset + 1]) << 16) |
           (static_cast<int32_t>(data[offset + 2]) << 8) |
           (static_cast<int32_t>(data[offset + 3]));
}

/**
 * @brief 计算给定字节序列的CRC校验和
 *
 * 计算从迭代器start到end之间的字节序列的CRC校验和。
 *
 * @param start 字节序列的起始迭代器
 * @param end 字节序列的结束迭代器
 *
 * @return 计算得到的CRC校验和
 */
uint8_t HardwareInterface::CalculateCRC(std::vector<uint8_t>::const_iterator start, 
                         std::vector<uint8_t>::const_iterator end) {
    uint8_t crc = 0;
    for (auto it = start; it != end; ++it) {
        crc ^= *it;
    }
    return crc;
}

/**
 * @brief 检查帧的循环冗余校验（CRC）是否匹配
 *
 * 此函数用于检查给定的帧的CRC值是否与预期值匹配。它通过对帧的每个字节进行异或运算，并将结果与提供的CRC值进行比较。
 *
 * @param frame 帧数据，类型为std::vector<uint8_t>
 * @param crc 预期的CRC值，类型为uint8_t
 *
 * @return 如果帧的CRC值与预期值匹配，则返回true；否则返回false
 */
bool HardwareInterface::CheckCRC(const std::vector<uint8_t>& frame, uint8_t crc) {
    uint8_t xor_sum = 0;
    for (const auto& b : frame) {
        xor_sum ^= b;
    }
    return xor_sum == crc;
}


/**
 * @brief 检查串口是否打开
 *
 * 该函数用于检查与STM32微控制器的串口连接是否处于打开状态。
 *
 * @return 如果串口连接已打开，则返回true；否则返回false。
 */
bool HardwareInterface::IsSerialOK() const {
    return __stm32_serial.isOpen();
}


/**
 * @brief 获取电机编码器计数
 *
 * 获取硬件接口中的电机编码器计数。
 *
 * @return EncoderCounts 返回电机编码器计数
 */
EncoderCounts HardwareInterface::GetMotorEncoderCounts() const {
    return __motor_encoder_counts;
}


HardwareStates HardwareInterface::GetHardwareStates() const {
    return __hardware_states;
}